﻿using System;
using System.Text;
using System.Collections;
using GHIElectronics.NETMF.System;
using GHIElectronics.NETMF.FEZ;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.Hardware;
using MLRobotic.Asserv.Tools;
using MLRobotic.Asserv.Drivers;
using Microsoft.SPOT;
using MLRobotic.IA.Tools;
using MLRobotic.IA;

namespace MLRobotic.Asserv
{
    public class Deplace
    {

        public static int codeurDroit = 0;
        private static int lastResetDroit = 0;
        public static int CodeurDroitPourObj
        {
            get { return codeurDroit - lastResetDroit; }
        }
        public static int codeurGauche = 0;
        private static int lastResetGauche = 0;
        public static int CodeurGauchePourObj
        {
            get { return codeurGauche - lastResetGauche; }
        }

        private static void ResetCodeursPourObj()
        {
            lastResetDroit = codeurDroit;
            lastResetGauche = codeurGauche;
        }

        public static double curvalDroit = 0;
        public static double curvalGauche = 0;
        public static double objAvance = 0;
        public static double objX = 0;
        public static double objY = 0;
        public static bool objEnAvant;
        public static double ObjTeta = 0;
        public static double curObjAvance = 0;
        public static double curObjAngle = 0;

        public static double ObjAvancement = 0;
        public static double ObjTetaRadian = 0;
        public static double ObjAvancementReal = 0;
        public static double ObjTetaRadianReal = 0;

        private static double acceleration = 45;//30;
        private static double accelerationAngle = 15;
        public static double currentVitesse = 0;
        private static double currentVitesseAngle = 0;
        private static double nbPasPente = 0;
        private static double nbPasPenteAngle = 0;
        public static double vitesseMax = 750;//1500; // = Vitesse desiree/Fréquence de PID
        public static double vitesseMaxAngle = 750; // = Vitesse desiree/Fréquence de PID

        public static double coefPasMM = 19.42;

        public static int freqPWM = 20000;
        public static double Xpos = 0;
        public static double Ypos = 0;
        public static double TetaPos = 0;
        public static long NbPasGcourant, NbPasDcourant, avancement;
        static long LastCodeurG = 0;
        static long LastCodeurD = 0;
        static long EntraxePas = 5845;//5375;
        static LS7366 LSCodeurGauche, LSCodeurDroit;
        static PWM pwmDroitTrigo, pwmDroitHoraire, pwmGaucheTrigo, pwmGaucheHoraire;

        private static OutputPort feuStopDroit, feuStopGauche, feuReculeDroit, feuReculeGauche, feuClignotantDroit, feuClignotantGauche, enableMotDroit, enableMotGauche;

        public static void Init()
        {
            LSCodeurGauche = new LS7366((Cpu.Pin)FEZ_Pin.Digital.Di20, SPI.SPI_module.SPI1);
            LSCodeurDroit = new LS7366((Cpu.Pin)FEZ_Pin.Digital.Di21, SPI.SPI_module.SPI1);
            pwmDroitTrigo = new PWM((PWM.Pin)FEZ_Pin.PWM.Di5);
            pwmDroitHoraire = new PWM((PWM.Pin)FEZ_Pin.PWM.Di6);
            pwmGaucheTrigo = new PWM((PWM.Pin)FEZ_Pin.PWM.Di9);
            pwmGaucheHoraire = new PWM((PWM.Pin)FEZ_Pin.PWM.Di10);

            enableMotDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di7, true);
            enableMotGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di8, true);

            // Init des feux
            feuStopDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An0, false);
            feuStopGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An3, false);
            feuReculeDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An2, false);
            feuReculeGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An5, false);
            feuClignotantDroit = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An1, false);
            feuClignotantGauche = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.An4, false);


            nbPasPenteAngle = vitesseMaxAngle * (vitesseMaxAngle / accelerationAngle) / 2;
            // on augmente un peu la pente car sinon l'inertie casse tout
            nbPasPente = vitesseMax * (vitesseMax / acceleration) / 2 * 1.15;
            PID.pidAvance = new PID(2.4, 0, 0.31, 4000, -4000, GetCurrentAvance, GetCurrentObjAvance, SetCommandAvance);
            PID.pidAngle = new PID(1.7, 0, 0.25, 4000, -4000, GetCurrentCodeursAngle, GetCurrentObjectiveAngle, SetCommandAngle);
            PID.Enable();
        }


        static double tP = 0.7;
        static double tI = 0;
        static double tD = 0;
        static DateTime dateDebutCalculPID = DateTime.MinValue;
        static bool premierDeplac = true;

        public static void CalculPID()
        {

            if (Deplace.ArriveACible && premierDeplac && tI < 3)
            {
                if (dateDebutCalculPID != DateTime.MinValue)
                {
                    Debug.Print(tP.ToString("N2") + ";" + tI.ToString("N2") + ";" + tD.ToString("N2") + ";" + dateDebutCalculPID.ToString("HH:mm:ss") + ";" + DateTime.Now.ToString("HH:mm:ss") + ";" + PID.pidAngle.calculPIDSommeErreur.ToString("N2"));
                }
                dateDebutCalculPID = DateTime.Now;

                tP += 0.1;
                tI += 0;
                tD += 0;

                PID.Disable();
                PID.pidAngle = null;
                PID.pidAvance = null;
                PID.pidAngle = new PID(1.7, 0, 0.25, 4000, -4000, GetCurrentAvance, GetCurrentObjAvance, SetCommandAvance);
                PID.pidAvance = new PID(tP, tI, tD, 4000, -4000, GetCurrentCodeursAngle, GetCurrentObjectiveAngle, SetCommandAngle);
                PID.Enable();

                // Pour test : next cible
                //GotoTeta(System.Math.PI);
                GotoXY(0, 10000, true);
                premierDeplac = false;
            }
            else if (Deplace.ArriveACible)
            {
                //GotoTeta(0);
                GotoXY(0, 0, false);
                premierDeplac = true;
            }


        }



        static bool feuxOn = false;
        private static double GetCurrentObjAvance()
        {
            // incrémentation ou décrément de l'acceleration
            if (currentVitesse < vitesseMax
                && Math2.Abs(objAvance) > Math2.Abs(curObjAvance))
                currentVitesse += acceleration;
            else if (ObjTeta == 0 && objAvance != 0 && (currentVitesse <= 0 || Math2.Abs(objAvance) >= Math2.Abs(ObjAvancementReal)))
            {
                currentVitesse = 0;
                objAvance = ObjAvancementReal;
            }
            else if (currentVitesse > 0
                && Math2.Abs(objAvance) <= Math2.Abs(curObjAvance))
            {
                // arret de la correction d'angle de destination
                objX = objY = 0;
                currentVitesse -= acceleration;
                AllumerFeuxStops();
                feuxOn = true;
            }

            if (feuxOn && currentVitesse == 0)
            {
                ArreterFeuxRecule();
                ArreterFeuxStops();
            }

            if (objAvance < 0)
                AllumerFeuxRecule();

            // on augmente de la vitesse
            curObjAvance += Math2.Sign(objAvance) * currentVitesse;

            return curObjAvance;
        }

        private static int curVitesseDroit, curVitesseGauche;
        private static double GetCurrentAvance()
        {
            int d = codeurDroit;
            int g = codeurGauche;

            codeurDroit = -LSCodeurDroit.ReadCounter();
            codeurGauche = LSCodeurGauche.ReadCounter();

            curVitesseDroit = System.Math.Abs(d - codeurDroit);
            curVitesseGauche = System.Math.Abs(g - codeurGauche);


            return (CodeurDroitPourObj + CodeurGauchePourObj) / 2;
        }
        private static void SetCommandAvance(double val)
        {
            // on ne settera les PWM qu'au set de l'angle (on fois qu'on aura la valeur finale)
            curvalDroit = curvalGauche = val;
        }

        // static string bufferLog = "";
        private static double GetCurrentObjectiveAngle()
        {
            // incrémentation ou décrément de l'acceleration
            if (Math2.Abs(currentVitesseAngle) < vitesseMaxAngle
                && Math2.Abs(ObjTeta * EntraxePas / 2) > Math2.Abs(curObjAngle))
            {
                currentVitesseAngle += accelerationAngle;
                AllumerClignotant(Math2.Sign(ObjTeta) > 0);
            }
            else if (currentVitesseAngle < 0 || (Math2.Abs(ObjTetaRadianReal * EntraxePas / 2) < Math2.Abs(curObjAngle)))
            {
                currentVitesseAngle = 0;
                curObjAngle = ObjTetaRadianReal * EntraxePas / 2;
            }
            else if (currentVitesseAngle > 0
               && Math2.Abs(ObjTeta * EntraxePas / 2) <= Math2.Abs(curObjAngle))
                currentVitesseAngle -= accelerationAngle;
            else if (Math2.Abs(objAvance) > 0 && ObjTeta == 0)
            {
                //bufferLog += "\r\nObjTetaRadianReal;" + ObjTetaRadianReal.ToString("F4") + ";" + TetaPos.ToString("F4");
                return ObjTetaRadianReal;
            }
            // cas de la correction d'angle lors de l'avance
            //else if (Math2.Abs(objAvance) > 0 && ObjTeta == 0)
            //{
            //  double  ObjAv = MathEx.Sqrt(System.Math.Pow(Math2.Abs(objX - Xpos), 2) + System.Math.Pow(Math2.Abs(objY - Ypos), 2));


            //  if (ObjAv > 5000)
            //    {
            //        int signe = MathEx.Asin((objY - Ypos) / ObjAv) > 0 ? 1 : -1;
            //        double Aacos = MathEx.Acos((objX - Xpos) / ObjAv);
            //        double dteta;

            //        if (objEnAvant)
            //            dteta = (Aacos * signe - (TetaPos % (2 * System.Math.PI)));
            //        else
            //            dteta = (Aacos * signe - ((TetaPos - System.Math.PI) % (2 * System.Math.PI)));

            //        if (dteta < (-System.Math.PI))
            //            dteta += 2 * System.Math.PI;
            //        else if (dteta > (System.Math.PI))
            //            dteta -= 2 * System.Math.PI;

            //        Debug.Print("DTETA : " + dteta.ToString("F4"));

            //        return -(dteta * EntraxePas / 2);

            //    }

            //}



            if (currentVitesseAngle == 0)
                ArreterClignotant();

            curObjAngle += Math2.Sign(ObjTeta) * currentVitesseAngle;

            // bufferLog += "\r\nObjTetaRadianReal;" + ObjTetaRadianReal.ToString("F4") + ";" + TetaPos.ToString("F4");


            return curObjAngle;
        }

        static bool clignotantDroit = false;
        static bool clignotantGauche = false;
        private static void ArreterClignotant()
        {
            clignotantDroit = false;
            clignotantGauche = false;
        }

        private static void AllumerClignotant(bool gauche)
        {
            clignotantDroit = !gauche;
            clignotantGauche = gauche;
        }

        private static void AllumerFeuxRecule()
        {
            feuReculeDroit.Write(true);
            feuReculeGauche.Write(true);
        }

        private static void AllumerFeuxStops()
        {
            feuStopDroit.Write(true);
            feuStopGauche.Write(true);
        }

        private static void ArreterFeuxStops()
        {
            feuStopDroit.Write(false);
            feuStopGauche.Write(false);
        }

        private static void ArreterFeuxRecule()
        {
            feuReculeDroit.Write(false);
            feuReculeGauche.Write(false);
        }
        private static double GetCurrentCodeursAngle()
        {
            //Debug.Print("ANGLE ================= " + CodeurGauchePourObj.ToString("F2") + ";" + CodeurDroitPourObj.ToString("F2"));
            return (CodeurGauchePourObj - CodeurDroitPourObj) / 2;
        }
        private static void SetCommandAngle(double val)
        {
            // -= car lié aux branchements...
            curvalGauche -= val;
            curvalDroit += val;
            //Debug.Print("SET ANGLE ================= " + curvalGauche.ToString("N2") + ";" + curvalDroit.ToString("N2"));

            // Set des PWM
            SetPWMValue(pwmGaucheTrigo, pwmGaucheHoraire, curvalGauche);
            SetPWMValue(pwmDroitTrigo, pwmDroitHoraire, curvalDroit);

            Debug.Print("ASSERV;" + TetaPos.ToString("F3") + ";" + curvalGauche.ToString("F3") + ";" + curvalDroit.ToString("F3") + ";" + GetDutyCycle(curvalGauche) + ";" + GetDutyCycle(curvalDroit));
        }

        private static void SetPWMValue(PWM pwmTrigo, PWM pwmHoraire, double val)
        {
            if (val >= 0)
            {
                pwmTrigo.Set(freqPWM, 0);
                pwmHoraire.Set(freqPWM, GetDutyCycle(val));

            }
            else
            {
                pwmTrigo.Set(freqPWM, GetDutyCycle(val));
                pwmHoraire.Set(freqPWM, 0);
            }
        }

        private static byte GetDutyCycle(double val)
        {
            // valeur max : 6000=93% (auquel on ajoutera 7)
            byte b = System.Convert.ToByte((Math2.Abs(val) / 50).ToString("N0"));

            return (byte)(7 + (byte)(b < 93 ? b : (byte)93));
        }


        public static bool isStopped = true;
        public static Queue posHistory = new Queue();
        static DateTime dateNextCalculPosition = DateTime.Now;
        static int tempoCalculPosition = 25;
        public static void CalculPosition()
        {
            if (DateTime.Now > dateNextCalculPosition)
            {
                // Temporisation entre les calculs de position
                dateNextCalculPosition = DateTime.Now.AddMilliseconds(tempoCalculPosition);

                //Recupération du nombre de pas parcouru depuis la dernière boucle
                NbPasGcourant = codeurGauche - LastCodeurG;
                NbPasDcourant = codeurDroit - LastCodeurD;

                posHistory.Enqueue(codeurGauche);
                posHistory.Enqueue(codeurDroit);

                if (posHistory.Count >= 8)
                {
                    // on est a l'arret si les codeur n'avance pas de plus de 6 pas entre 5 ticks
                    long val = Math2.Abs((int)posHistory.Dequeue() - codeurGauche);
                    val += Math2.Abs((int)posHistory.Dequeue() - codeurDroit);
                    isStopped = currentVitesse == 0 && currentVitesseAngle == 0 && (val < 200);

                }

                LastCodeurG += NbPasGcourant;
                LastCodeurD += NbPasDcourant;


                TetaPos += (double)(NbPasGcourant - NbPasDcourant) / (double)EntraxePas;

                //calcul de l avancement effectue
                avancement = (NbPasGcourant + NbPasDcourant) / 2;

                //Mise a jour des positions
                Xpos += ((double)avancement * MathEx.Cos(TetaPos));
                Ypos += ((double)avancement * MathEx.Sin(TetaPos));

                Debug.Print("POSITION;" + Xpos.ToString("F2") + ";" + Ypos.ToString("F2") + ";" + TetaPos.ToString("F4") + ";" + NbPasGcourant.ToString("F2") + ";" + NbPasDcourant.ToString("F2") + ";" + codeurGauche.ToString("F2") + ";" + codeurDroit.ToString("F2"));
            }
        }

        public static void GotoXY(double X, double Y, bool enAvant)
        {
            X *= coefPasMM;
            Y *= coefPasMM;

            objX = X;
            objY = Y;
            objEnAvant = enAvant;

            ObjAvancement = MathEx.Sqrt(System.Math.Pow(Math2.Abs(X - Xpos), 2) + System.Math.Pow(Math2.Abs(Y - Ypos), 2));


            if (ObjAvancement != 0)
            {
                int signe = MathEx.Asin((Y - Ypos) / ObjAvancement) > 0 ? 1 : -1;
                double Aacos = MathEx.Acos((X - Xpos) / ObjAvancement);

                if (enAvant)
                    ObjTetaRadian = (Aacos * signe - (TetaPos % (2 * System.Math.PI)));
                else
                    ObjTetaRadian = (Aacos * signe - ((TetaPos - System.Math.PI) % (2 * System.Math.PI)));



                if (ObjTetaRadian < (-System.Math.PI))
                    ObjTetaRadian += 2 * System.Math.PI;
                else if (ObjTetaRadian > (System.Math.PI))
                    ObjTetaRadian -= 2 * System.Math.PI;


                ObjTetaRadianReal = ObjTetaRadian;

                // suppression de l'angle du a la pente
                if (Math2.Abs(ObjTetaRadian) > 2 * nbPasPenteAngle / EntraxePas)
                    ObjTetaRadian += nbPasPenteAngle / EntraxePas * (ObjTetaRadian < 0 ? 1 : -1);
                else
                    ObjTetaRadian = ObjTetaRadian / 2;
            }
            else
                ObjTetaRadian = 0;


            ObjAvancementReal = ObjAvancement;
            // suppression de l'angle du a la pente
            if (Math2.Abs(ObjAvancement) > 2 * nbPasPente)
                ObjAvancement += nbPasPente * (ObjAvancement < 0 ? 1 : -1);
            else
                // on augmente un peu la pente car sinon l'inertie casse tout
                ObjAvancement = ObjAvancement / 2.05;

            if (!enAvant)
            {
                // on recule : avancement négatif
                ObjAvancement = -ObjAvancement;
                ObjAvancementReal = -ObjAvancementReal;
            }
            SDCard.Write("GOTO XY : " + X.ToString("F1") + ";" + Y.ToString("F1") + ";" + enAvant.ToString() + ";" + ObjTetaRadianReal.ToString() + ";" + ObjAvancementReal.ToString());

        }


        //reception d une commande de déplacement en radian en absolue
        public static void GotoTeta(double gotoTeta)
        {
            SDCard.Write("GOTO TETA : " + gotoTeta.ToString("F4"));

            // calcul de l'objectif par rapport à la position actuelle
            ObjTetaRadianReal = ObjTetaRadian = (gotoTeta - TetaPos);



            // suppression de l'angle du a la pente
            if (Math2.Abs(ObjTetaRadian) > 2 * nbPasPenteAngle / EntraxePas)
                ObjTetaRadian += nbPasPenteAngle / EntraxePas * (ObjTetaRadian < 0 ? 1 : -1);
            else
                ObjTetaRadian = ObjTetaRadian / 2;
        }

        public static void  Stop()
        {
            SDCard.Write("GOTO STOP");

            //curObjAvance = (CodeurDroitPourObj + CodeurGauchePourObj) / 2;
            //ObjTetaRadianReal = curObjAngle = (CodeurGauchePourObj - CodeurDroitPourObj) / 2;


            ObjAvancementReal = ObjAvancement = 0;
            ObjTeta = curObjAngle * 2 / EntraxePas;
            objAvance = curObjAvance;
            ArriveACible = false;

        }

        public static bool ArriveACible = true;
        public static void DeplaceBouge()
        {
            bool arret = true;
            arret = (ObjTeta != 0 && currentVitesseAngle == 0)
                || (objAvance != 0 && currentVitesse == 0)
                || (objAvance == 0 && ObjTeta == 0);

            //Si on est a l arret et que l on a un objectif en teta --> on tourne
            if (isStopped && arret && (ObjTetaRadian != 0))
            {

                ObjTeta = ObjTetaRadian;
                ObjTetaRadian = 0;

                SDCard.Write("GO TETA : " + DateTime.Now.ToString("ss.fff") + ";" + ObjTeta.ToString() + ";" + Xpos.ToString() + ";" + Ypos.ToString() + ";" + TetaPos.ToString());

                Program.comPort.Send(IA.Drivers.COM.typePacket.RetourDebutRotation);

                ArriveACible = false;

                currentVitesseAngle += acceleration;

                PID.Reset();


                ResetCodeursPourObj();



                //LastCodeurD = LastCodeurG = codeurDroit = codeurGauche = 0;
                curObjAngle = curObjAvance = objAvance = 0;

            }
            // Sinon si on est à l'arret et que l'on a un objectif en distance
            else if (isStopped && arret && (ObjAvancement != 0))
            {
                objAvance = ObjAvancement;
                ObjAvancement = 0;


                Program.comPort.Send(IA.Drivers.COM.typePacket.RetourFinRotation);


                Debug.Print("GO Avance : " + objAvance.ToString());
                //Log.Write("IN TETA : " + ObjTetaRadianReal.ToString() + ";" + Xpos.ToString() + ";" + Ypos.ToString() + ";" + TetaPos.ToString());

                ArriveACible = false;

                currentVitesse += acceleration;

                PID.Reset();

                ObjTetaRadianReal = (ObjTetaRadianReal * EntraxePas / 2) - ((CodeurGauchePourObj - CodeurDroitPourObj) / 2);
                SDCard.Write("GO AVANCE : " + DateTime.Now.ToString("ss.fff") + ";" + objAvance.ToString() + ";" + Xpos.ToString("F2") + ";" + Ypos.ToString("F2") + ";" + TetaPos.ToString("F4") + ";" + ObjTetaRadianReal.ToString("F4"));

                ResetCodeursPourObj();
                //LastCodeurD = LastCodeurG = codeurDroit = codeurGauche = 0;
                ObjTeta = curObjAngle = curObjAvance = 0;

            }
            else if (isStopped && arret && !ArriveACible)
            {
                Program.comPort.Send(IA.Drivers.COM.typePacket.RetourFinRotation);

                // Log.Write("ARRIVE : " + DateTime.Now.ToString("ss.fff") + ";" + Xpos.ToString("F2") + ";" + Ypos.ToString("F2") + ";" + TetaPos.ToString("F4") + ";" + ObjTetaRadianReal.ToString("F4"));
                //Debug.Print("ARRET ! Arrivé");
                if (!alertDerapage)
                    Program.comPort.SendPosition(IA.Drivers.COM.typePacket.RetourPositionFinDeplace);
                else
                {
                    Program.comPort.SendPosition(IA.Drivers.COM.typePacket.RetourPositionBlocageRobot);
                    alertDerapage = false;
                }

                ArriveACible = true;

                objAvance = 0;
            }

            //// gestion du dérapage
            //if ((Math2.Abs(curvalGauche) > 3000 && System.Math.Abs(curVitesseGauche) < 20) ||
            //    (Math2.Abs(curvalDroit) > 3000 && System.Math.Abs(curVitesseDroit) < 20))
            //    countDerapage++;
            //else
            //    countDerapage = 0;

            //if (countDerapage >= 7)
            //{
            //    Debug.Print("BLOCAGE !!!!!!!");
            //    alertDerapage = true;
            //    Stop();
            //}

            // Gestion des clignotants
            if (nextCligno < DateTime.Now)
            {
                feuClignotantDroit.Write(clignotantDroit && clignoOn);
                feuClignotantGauche.Write(clignotantGauche && clignoOn);

                clignoOn = !clignoOn;
                nextCligno = DateTime.Now.AddMilliseconds(200);
            }
        }
        static int countDerapage = 0;
        static bool alertDerapage = false;
        static bool clignoOn = false;
        static DateTime nextCligno = DateTime.Now;
    }
}